import threading
import time
import socket
import pygame
import cv
import Image
import io
import Queue
import StringIO

SERVER="localhost"

class Controll(threading.Thread):

    def __init__(self, ip, port, timeout, ErrorQueue):
        threading.Thread.__init__(self)
        self.Ip=ip
        self.Port=port
        self.Timeout=timeout
        self.ErrorQueue=ErrorQueue
        self.Die=False
        self.Joystick=Controll.Joystick(self.ErrorQueue)
        self.Keyboard=Controll.Keyboard(self.ErrorQueue)

    def Send(self):
        last=chr(254)+chr(1)+chr(127)+chr(254)+chr(2)+chr(127)
        while not self.Die:
            data=""
            try:
                data=self.Joystick.Queue.get(timeout=0.1)
            except:
                data=last
            time.sleep(0.01)
            try:
                self.Socket.send(data)
            except:
                self.Socket.close()
                self.connecting()
            while not self.ServerFeedBack.Queue.empty():
                temp=self.ServerFeedBack.Queue.get()
                self.ErrorQueue.put(temp)                
            last=data
        
    def connecting(self):
        self.Socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.Socket.settimeout(self.Timeout)
        self.ServerFeedBack=Controll.ServerFeedBack(self.Socket, self.ErrorQueue)
        while not self.Die:
            try:
                self.Socket.connect((self.Ip, self.Port))
                self.ServerFeedBack.start()
                break
            except:
                self.ErrorQueue.put("Controll: ERROR: Could not Connect to %s at Port %d!"%(self.Ip,self.Port))
                time.sleep(1)
                
    def run(self):
        self.Keyboard.start()
        self.Joystick.start()
        self.connecting()
        self.Send()
        self.Joystick.Die=True
    
    class Keyboard(threading.Thread):
        def __init__(self, ErrorQueue):
            threading.Thread.__init__(self)
            self.ErrorQueue=ErrorQueue
            self.Queue=Queue.Queue()   
            self.Die=False
        def run(self):
            pygame.init()
            while True:
                for e in pygame.event.get():
                    print e
        
    class Joystick(threading.Thread):
        def __init__(self, ErrorQueue):
            threading.Thread.__init__(self)
            self.ErrorQueue=ErrorQueue
            self.Queue=Queue.Queue()   
            self.Die=False
            self.DEAD=0.1
            self.Motor1=127
            self.Motor2=127
            self.CamX=0.0
            self.CamY=0.0

        def calcpos(self, pt):
            
            xo= (pt[0])
            yo= (pt[1])
            x=yo
            y=yo
            if yo >= 0:
                if xo >= 0:
                    y+=-xo
                    if y > 0: x=xo+y
                    else: x=xo
                else:
                    x+=xo
                    if x > 0: y=yo
                    else: y=yo-x
            else:
                if xo >= 0:
                    x+=xo
                    if x > 0:
                        x=xo+yo
                        y=-xo
                else:
                    y=-xo+yo
                    if y > 0: x=xo
            return (x*-1,y*-1)

        def minmax(self, value, max, min):
            if value > max:
                return max
            if value < min:
                return min
            return value

        def EnableJoystick(self, nr):
            while not self.Die:
                try:
                    pygame.init()
                    pygame.joystick.init()
                    self.joystick=pygame.joystick.Joystick(nr)
                    self.joystick.init()
                    self.ErrorQueue.put('Enabled joystick: %s'%self.joystick.get_name())
                    break
                except pygame.error:
                    self.ErrorQueue.put("Controll: No Joystick found!")
                    time.sleep(1)

        def GetJoystickEvents(self):
            ret=""
            
            for e in pygame.event.get():
                ret=""
                if e.type == 7: #joyaxis
                    
                    x , y = self.joystick.get_axis(0), self.joystick.get_axis(1)
                    self.CamX , self.CamY = self.joystick.get_axis(4), self.joystick.get_axis(3)
                    acc=-self.joystick.get_axis(2)
                    if (acc > self.DEAD or acc < -self.DEAD):
                        if x > self.DEAD:
                            self.Motor1=127.0 + acc * 127.0
                            self.Motor2=127.0 + acc * ( 1 - x ) * 127.0					
                        elif x < -self.DEAD:
                            self.Motor1=127.0 + acc * (1+x) * 127.0
                            self.Motor2=127.0 + acc * 127.0
                        else:
                            self.Motor1=127.0+acc*127.0
                            self.Motor2=127.0+acc*127.0
                    elif (x>self.DEAD or x<-self.DEAD or y>self.DEAD or y<-self.DEAD):
                        #print "no gas"
                        (x, y) = self.calcpos((x,y))
                        self.Motor1=127.0+y*127.0
                        self.Motor2=127.0+x*127.0
                    self.Motor1=self.minmax(self.Motor1, 250, 5)
                    self.Motor2=self.minmax(self.Motor2, 250, 5)
                    ret=""
                    if self.Motor1 > 140 or self.Motor1< 110:
                        self.Queue.put(chr(254) + chr(1) + chr(int(self.Motor1)))
                    if self.Motor2 > 140 or self.Motor2 < 110:
                        self.Queue.put(chr(254) + chr(2) + chr(int(self.Motor2)))
                    self.Queue.put(chr(253)+chr(1)+chr(int(127.0+self.CamY*127.0)) + chr(253)+chr(2)+chr(int(127.0+self.CamX*127.0)))
                elif e.type == 10: #Button down
                    for i in range(self.joystick.get_numbuttons()):
                        if self.joystick.get_button(i):
                            print "Button %d pressed"%i
                            if i==0: #A
                                pass
                            elif i==1: #B
                                pass
                            elif i==2: #X
                                pass
                            elif i==3: #Y
                                pass
                            elif i==4: #LB
                                pass
                            elif i==5: #RB
                                pass
                            elif i==6: #Back
                                pass
                            elif i==7: #Start
                                pass
        def run(self):
            self.EnableJoystick(0)
            while not self.Die:
                if self.Queue.empty():
                    self.GetJoystickEvents()


    class ServerFeedBack(threading.Thread):
            def __init__(self, connection, ErrorQueue):
                threading.Thread.__init__(self)
                self.Connection=connection
                self.ErrorQueue=ErrorQueue
                self.Die=False
                self.Queue=Queue.Queue()
                
            def run(self):
                while not self.Die:
                    try:
                        data=self.Connection.recv(1024)
                    except:
                        break
                    if self.Queue.empty():
                        self.Queue.put(data)
                exit()            





class ShowFrame(threading.Thread):
    def __init__(self, ErrorQueue):
        threading.Thread.__init__(self)
        self.ErrorQueue=ErrorQueue
        self.Die=False
        self.PicMode=1
        self.ImageSize=(640,480)
        self.Resize=(800,600)
        self.FrameReception=ShowFrame.FrameReception(SERVER, 2002 , 5, self.ErrorQueue)
        self.Frame = cv.CreateImage(self.Resize, 8, self.PicMode)
        self.FramesDropped=0

    def jpeg2cvFrame(self, cvframe, jpeg, resolution, mode, resize):
        input=StringIO.StringIO()
        input.write(jpeg)
        input.seek(0)
        im=Image.open(input)
        if resolution!=resize:
            im=im.resize(resize, Image.BILINEAR)
        cv.SetData(cvframe, im.tostring(), resize[0]*self.PicMode)

    
    def run(self):
        self.FrameReception.start()
        timer=time.clock()
        Error=[]
        self.font=cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5,0.5,0,1)
        while not self.Die:
            try:
                jpeg=self.FrameReception.FrameQueue.get(timeout=1)
            except:
                self.Frame = cv.CreateImage(self.Resize, 8, self.PicMode)
                self.ErrorQueue.put("ERROR Receiving Frames")
            try:
                self.jpeg2cvFrame(self.Frame, jpeg, self.ImageSize, 8, self.Resize)
            except:
                self.FramesDropped+=1
            i=0
            while not self.ErrorQueue.empty():
                temp=self.ErrorQueue.get()
                if not temp in Error:
                    timer=time.clock()
                    Error.append(temp)
            
            for i in range(len(Error)):
                if "ERROR" in Error[i]:
                    cv.PutText(self.Frame, Error[i], (self.Resize[0]/100*1,self.Resize[1]/100*(80+(i*4))), self.font, cv.CV_RGB(255,0,0))
                elif "QUALITY" in Error[i]:
                    self.FrameReception.ComandQueue.put(Error[i])
                else:
                    cv.PutText(self.Frame, Error[i], (self.Resize[0]/100*1,self.Resize[1]/100*(80+(i*4))), self.font, cv.CV_RGB(0,255,127))
                    

            if time.clock()-timer > 2:
                #print time.clock()
                timer=time.clock()
                for elem in Error:
                    Error.remove(elem)
    
            cv.ShowImage( "CamShiftDemo", self.Frame )
            c = cv.WaitKey(7)
            if c == 100: #"L"
                self.FrameReception.ComandQueue.put("PICMODE:L")
                self.PicMode=1
                self.Frame = cv.CreateImage(self.Resize, 8, self.PicMode)
                for _ in range(25):
                    temp=self.FrameReception.FrameQueue.get(timeout=1)
                print "L"
            if c == 114: #"R"
                self.FrameReception.ComandQueue.put("PICMODE:RGB")
                self.PicMode=3
                self.Frame = cv.CreateImage(self.Resize, 8, self.PicMode)
                for _ in range(25):
                    temp=self.FrameReception.FrameQueue.get(timeout=1)
                print "RGB"
            if c == 43: #"+"
                self.FrameReception.ComandQueue.put("QUALITY:90")
            if c == 45: #"-"
                self.FrameReception.ComandQueue.put("QUALITY:20")
            if c == 27: #"Esc"
                break
        self.FrameReception.Die=True
        
    class FrameReception(threading.Thread):
        def __init__(self, ip, port, timeout, ErrorQueue):
            threading.Thread.__init__(self)
            self.Ip=ip
            self.Port=port
            self.Timeout=timeout
            self.ErrorQueue=ErrorQueue
            self.FrameQueue=Queue.Queue()
            self.ComandQueue=Queue.Queue()
            self.Die=False
            self.Buffer=""
            self.ImageSize=(640,480)

        def connecting(self):
            self.Socket=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            while not self.Die:
                try:
                    self.Socket.connect((self.Ip, self.Port))
                    break
                except:
                    self.ErrorQueue.put("Video: Could not Connect to %s at Port %d!"%(self.Ip,self.Port))
                    time.sleep(1)
                    
        def run(self):
            self.connecting()
            print "FrameReception: Connected"
            while not self.Die:
                try:
                    self.data=self.Socket.recv(self.ImageSize[0]*self.ImageSize[1]*3)
                except:
                    self.connecting()

                while not self.ComandQueue.empty():
                    self.Socket.send(self.ComandQueue.get())

                if self.data:
                    if self.FrameQueue.empty():
                        self.FrameQueue.put(self.data)
                else:
                    self.connecting()
                
class Client(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.ErrorQueue=Queue.Queue()
        self.ShowFrame=ShowFrame(self.ErrorQueue)
        self.Controll=Controll(SERVER , 2001, 5, self.ErrorQueue)
    def run(self):
        self.ShowFrame.start()
        self.Controll.start()
        self.ShowFrame.join()
        self.Controll.Die=True
        

    
if __name__ == "__main__":
    oClient=Client()
    oClient.start()
    oClient.join()

    exit()
